#include "opencv2/core/core.hpp"
#include "opencv2/video/background_segm.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>
#include "cv.h"
#include "cxcore.h"
#include "math.h"

using namespace std;
using namespace cv;

void help()
{
 printf("\nDo background segmentation, especially demonstrating the use of cvUpdateBGStatModel().\n"
"Learns the background at the start and then segments.\n"
"Learning is togged by the space key. Will read from file or camera\n"
"Usage: \n"
"			./bgfg_segm [--camera]=<use camera, if this key is present>, [--file_name]=<path to movie file> \n\n");
}

const char* keys = 
{
	"{c |camera   |false    | use camera or not}"
	"{fn|file_name|tree.avi | movie file             }"
};

class atsBlobFinder
{
public:
	atsBlobFinder()
	{
	}
	///Original Code by http://nghiaho.com/?p=1102
	///Changed and added commments. Removed Errors
	///works with VS2010 and OpenCV 2.2+
	void FindBlobs(const cv::Mat &binary, vector < vector<cv::Point>  > &blobs)
	{
		blobs.clear();
		
		// Fill the label_image with the blobs
		// 0  - background
		// 1  - unlabelled foreground
		// 2+ - labelled foreground
		
		///input is a binary image therefore values are either 0 or 1
		///out objective is to find a set of 1's that are together and assign 2 to it
		///then look for other 1's, and assign 3 to it....so on a soforth
		
		cv::Mat label_image;
		binary.convertTo(label_image, CV_32FC1); // weird it doesn't support CV_32S! Because the CV::SCALAR is a double value in the function floodfill
		
		int label_count = 2; // starts at 2 because 0,1 are used already
		
		//erode to remove noise-------------------------------
		int elem_size = 1*1+1;
		Mat element_erode = getStructuringElement( MORPH_RECT,
											Size( elem_size, elem_size ),
											Point( 0, 0 ) );
		Mat element_dilate = getStructuringElement( MORPH_RECT,
											Size( elem_size, elem_size ),
											Point( 0, 0 ) );
		
		/// Apply the erosion operation
		erode( label_image, label_image, element_erode);
		
		dilate(label_image, label_image, element_dilate);
		//---------------------------------------------------
		
		//just check the Matrix of label_image to make sure we have 0 and 1 only
		//cout << label_image << endl;
		
		for(int y=0; y < binary.rows; y++) {
			for(int x=0; x < binary.cols; x++) {
				float checker = label_image.at<float>(y,x); //need to look for float and not int as the scalar value is of type double
				cv::Rect rect;
				//cout << "check:" << checker << endl;
				if(checker == 1) {
					//fill region from a point
					cv::floodFill(label_image, cv::Point(x,y), cv::Scalar(label_count), &rect, cv::Scalar(0), cv::Scalar(0), 4);
					label_count++;
					//cout << label_image << endl <<"by checking: " << label_image.at<float>(y,x) <<endl;
					//cout << label_image;
					
					//a vector of all points in a blob
					std::vector<cv::Point> blob;
					
					for(int i=rect.y; i < (rect.y+rect.height); i++) {
						for(int j=rect.x; j < (rect.x+rect.width); j++) {
							float chk = label_image.at<float>(i,j);
							//cout << chk << endl;
							if(chk == label_count-1) {
								blob.push_back(cv::Point(j,i));
							}                        
						}
					}
					//place the points of a single blob in a grouping
					//a vector of vector points
					blobs.push_back(blob);
					//cout << "found blobl";
				}
			}
		}
		//cout << label_count <<endl;
		imshow("label image",label_image);
	}
private:
}; 


//this is a sample for foreground detection functions
int main(int argc, const char** argv)
{
	help();

	CommandLineParser parser(argc, argv, keys);
	bool useCamera = parser.get<bool>("camera");
	string file = parser.get<string>("file_name");
	VideoCapture cap("video_sam.mp4");
//	VideoCapture cap(0);
    bool update_bg_model = true;
/*
    if( useCamera )
        cap.open(0);
    else
		cap.open(file.c_str());
	parser.printParams();

    if( !cap.isOpened() )
    {
        printf("can not open camera or video file\n");
        return -1;
    }*/
    
    namedWindow("image", CV_WINDOW_NORMAL);
    namedWindow("foreground mask", CV_WINDOW_NORMAL);
    namedWindow("foreground image", CV_WINDOW_NORMAL);
    namedWindow("mean background image", CV_WINDOW_NORMAL);
    namedWindow("median blur", CV_WINDOW_NORMAL);
	cvNamedWindow("canny",0);
	cvResizeWindow("canny",200,200);

    BackgroundSubtractorMOG2 bg_model;
    Mat img, fgmask, fgimg;
	int counter = 0;
	
    for(;;)
    {
		
		counter = (counter+1)%7;
        cap >> img;
        
        if( img.empty() )
            break;
        
        if( fgimg.empty() )
          fgimg.create(img.size(), img.type());

        //update the model
        bg_model(img, fgmask, update_bg_model ? -1 : 0);

        fgimg = Scalar::all(0);
        img.copyTo(fgimg, fgmask);

        Mat bgimg;
        
		if (counter == 0) {
			bg_model.getBackgroundImage(bgimg);
		}

		Mat destination2 = cvCreateMat(fgmask.rows/10,fgmask.cols/10, CV_8UC3);
			resize(img, destination2, Size(), 0.5, 0.5, INTER_NEAREST);
        imshow("image", destination2);
			resize(fgmask, destination2, Size(), 0.5, 0.5, INTER_NEAREST);
        imshow("foreground mask", destination2);
			resize(fgimg, destination2, Size(), 0.5, 0.5, INTER_NEAREST);
        imshow("foreground image", destination2);

		if(!bgimg.empty()){
			resize(bgimg, destination2, Size(), 0.5, 0.5, INTER_NEAREST);

          imshow("mean background image", destination2 );
		}
		//Start contours detection
		Mat contours_mask(img.size(), CV_8UC4);
		Mat contours_edge(img.size(), CV_8UC4);
		threshold(fgmask, contours_mask,10, 255, THRESH_BINARY);
	//	medianBlur(contours_mask, contours_mask, 27);
		imshow("median blur", contours_mask);
		contours_mask.copyTo(contours_edge);
		Canny(contours_edge, contours_edge, 1, 3, 5);
		int percent = 10;
		Mat destination = cvCreateMat(contours_edge.rows/10,contours_edge.cols/10, CV_8UC3);
		resize(contours_edge, destination, Size(), 0.5, 0.5, INTER_NEAREST);

		/// ****************** trying connected components

		atsBlobFinder blb;
		cv::Mat output = cv::Mat::zeros(img.size(), CV_8UC3); //3 Channels
		
		Mat binary;
		binary = fgmask;
//cv::cvtColor(fgmask,binary,CV_BGR2GRAY); //Convert image to GrayScale
   //     binary = binary>128; //create the binary image	
		threshold(binary, binary, 128, 1, THRESH_BINARY);
		vector <vector<cv::Point> > blobs;
		
		//cout << binary;
		blb.FindBlobs(binary, blobs);
		for(size_t i=0; i < blobs.size(); i++) {
			unsigned char r = 255 * (rand()/(1.0 + RAND_MAX));
			unsigned char g = 255 * (rand()/(1.0 + RAND_MAX));
			unsigned char b = 255 * (rand()/(1.0 + RAND_MAX));
			
						
			for(size_t j=0; j < blobs[i].size(); j++) {
				int x = blobs[i][j].x;
				int y = blobs[i][j].y;
				
				output.at<Vec3b>(y,x)[0] = r; //channel 1
				output.at<Vec3b>(y,x)[1] = g; //channel 2
				output.at<Vec3b>(y,x)[2] = b; //channel 3
			}
		}
		imshow("con comp",output);
		
		
		///
		
		
		
		//use cvResize to resize source to a destination image
		
		imshow("canny ", destination);
        char k = (char)waitKey(30);
    
		if( k == 27 ) break;
        if( k == ' ' )
        {
            update_bg_model = !update_bg_model;
            if(update_bg_model)
            	printf("Background update is on\n");
            else
            	printf("Background update is off\n");
        }

		
    }

    return 0;
}
